#include "pid.h"

PID pid;

/************************************************
函数名称 ： PID_Init
功    能 ： pid数值初始化
*************************************************/
void PID_Init()
{
    pid.kp = 7.5;
    pid.kd = 22.5;
}

/************************************************
函数名称 ： pid_ctrl
功    能 ： pid计算
参    数 ： *pid ------ 指针
返 回 值 ： pid输出数值
*************************************************/
void pid_ctrl(PID *pid)
{
    pid->error = pid->setpoint - pid->current;

    pid->output = pid->kp * pid->error +
                  pid->kd * (pid->error - pid->lasterror) ;
    pid->lasterror = pid->error;
}


/************************************************
函数名称 ： Walk_go
功    能 ： 控制电机旋转
参    数 :  
            Speed  --- 电机转速(期望值)
*************************************************/
void Walk_go(float Speed)
{
    static int pwm_out1, pwm_out2;

    PID *pid_walk = &pid;

    pid_walk->setpoint = Speed;
    pid_walk->current = encoder_value;
    pid_ctrl(pid_walk);

    // printf("%.02lf,%d\r\n", Speed * 50, encoder_value * 50);

    pwm_out1 = (pid_walk->output > 0) ? pid_walk->output : 0;
    pwm_out2 = (pid_walk->output < 0) ? -pid_walk->output: 0;

    pwm_out1 = (pwm_out1 > 100) ? 100 : 0;
    pwm_out2 = (pwm_out2 > 100) ? 100 : 0;

    TIM3->CCR1 = pwm_out1;
    TIM3->CCR2 = pwm_out2;
}